#!/usr/bin/env python
# coding=utf-8
import os
import cv2

# import rospy
import pyrealsense2 as rs
import numpy as np
from ros_intrinsic import Ros
#from Camrea import Camrea
from Calibration import Calibration
from CameraClient import CameraClient


def main():
    camera = CameraClient()
    save_file = True
    camera_ip = "192.168.3.100"
    
    if not camera.connect(camera_ip):
        exit(-1)
    # ros节点终止时，或按下Esc退出采集
    while True:
        # 从相机中获取一组帧数据，提取出color帧，转换成opencv的arr格式，最终显示在图像显示窗口中
        # color_image = cam.get_frame()
        color_image = camera.captureColorImg()
        cv2.imshow('camera image', color_image)
        # 在50毫秒的周期内获取键盘输入，如果没有键盘输入，则直接进入下一轮数据采集
        # 如果有键盘输入，则在输入【Esc】时，退出数据采集；在输入【Enter】时，对当前图像进行处理
        kvalue = cv2.waitKey(50) & 0xFF
        if kvalue == 27:
            break
        elif kvalue == 10 or kvalue == 13:
            ret, corners = calib.findCirclesGrid(color_image)
            if ret == True:
                calib.show(color_image, ret, corners)
    cv2.destroyAllWindows()
    # cam.release()  # 相机释放
    if calib.circleCenters_nums < 4:
        print('Calibration need more data !')
    else:
        # 使用opencv提供的相机标定函数doublecalibrateCamera(args......)，求解相机的内参矩阵和畸变系数
        # 标定函数的返回值为重投影均方根误差
        result = calib.calibrateCamera(calibrationInfoUrl)
        if result:
            print "calibrateCamera finshed"


if __name__ == '__main__':
    ROS = Ros()
    calibrationInfoUrl = ROS.ros_init()
    # cam = Camrea()
    calib = Calibration()
    main()
